#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "protocol.h"
#include "communication.h"
#include "sensor_msgs/LaserScan.h"
#include "robot_msgs/RobotPosition.h"
#include "kinematic.h"

typedef const boost::function<void (const geometry_msgs::Twist::ConstPtr& msg)> InsCallback;


/**
 * do work.
 */
void do_pos(ros::Publisher &pub, U8 x, U8 y, U8 z){
	
	robot_msgs::RobotPosition pos_msg;
	pos_msg.sensor_id = 0;
	pos_msg.x = x;
	pos_msg.y = y;
	pos_msg.z = z;
	pub.publish(pos_msg);
}

void do_ultrasonic(ros::Publisher &pub, U8 range, U8 angle){
	std::cout<<"ultrasonic : range="
			 <<range<<",angle="
			 <<angle<<std::endl;
}


/**
 * This callback function will forward Institution message to our Robot.
 */
void InsForward(const geometry_msgs::Twist::ConstPtr& msg, Communication &com)
{
    ROS_INFO("Twist> [T=%f,W=%f]", msg->linear.x, msg->angular.z);
    U8 x[2] = {0}; // move at the x-axis of car, move right
    U8 y[2] = {0}; // move at the y-axis of car, move foward
    U8 z[2] = {0}; // rotation, 
    U8 orientation = 0;
    
    /** 注意：小车的x轴对应于Twist的y轴，小车的y轴对应于Twist的x轴。小车的y轴对应于前进速度。*/
    kinematic::U16 move_foward = kinematic::LinearVelocityToPulse(msg->linear.x);
    kinematic::U16 move_right = kinematic::LinearVelocityToPulse(msg->linear.y);
    kinematic::U16 rotation = kinematic::RotationVelocityToPluse(msg->angular.z);
    kinematic::U8  direction = kinematic::ComputeDirection(msg->linear.y
                                        , msg->linear.x, msg->angular.z);

    y[0] = move_foward/255;
    y[1] = move_foward%255;
    x[0] = move_right/255;
    x[1] = move_right%255;
    z[0] = rotation/255;
    z[1] = rotation%255;
    orientation = direction;
    
    ROS_INFO("Car> [ %x %x %x %x %x %x %x]", x[0], x[1]
                                           , y[0], y[1]
                                           , z[0], z[1]
                                           ,orientation);
	if(!com.SendCmdWithArgs( kPosMode, x, y ,z, orientation)){
		std::cerr<<"Error: Send Insititution failed"<<std::endl;
	}
}


int main(int argc, char **argv)
{
    /**
     * Init Node.
     */
    ros::init(argc, argv, "robot_node");
    ros::NodeHandle n;

    /**
     * deal with Institution message.
     */
    Communication com("192.168.1.1",2001);
    InsCallback ins_callback = boost::bind(InsForward, _1, boost::ref(com));
    ros::Subscriber sub = n.subscribe("/cmd_vel", 100, ins_callback);

    /**
     * deal with Sensors message.
     */
    ros::Publisher laser_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);
    ros::Publisher pos1_pub = n.advertise<robot_msgs::RobotPosition>("robot_pos1", 50);
    ros::Publisher pos2_pub = n.advertise<robot_msgs::RobotPosition>("robot_pos2", 50);
    //ros::Publisher imu_pub = ;
    ros::spin();
    while(ros::ok()){
        /** Receive data from robot */
        U8 type;
        U8 sensor_data[10];
        ros::spinOnce();
        if(!com.ReceiveData(type, sensor_data, 10)){
			std::cerr<<"Error:Receive sensor data failed"<<std::endl;
			continue;
		}
		
		/** Valid data */
        if(false){
            continue;
        }
        
        /** Foward to the topic */
	    if( type==0x03){ /** Ultrasonic data */
			do_ultrasonic(laser_pub, sensor_data[0], sensor_data[1]);
			
		}else if(type==0x88){ /** Position data */
			U8 sensor_id = sensor_data[0];
			if(sensor_id==0x01){
				do_pos(pos1_pub, sensor_data[1], sensor_data[2]
					, sensor_data[3]);
			}
			if(sensor_id==0x02){
				do_pos(pos2_pub, sensor_data[1], sensor_data[2]
					, sensor_data[3]);
			}
		}
    }
 
   return 0;
}
